openpilot Toyota: improve longitudinal control

As usual, don't have a route as I don't have access to comma servers (repaired device), but this works really well on my TSS-P Prius, best longitudinal I've seen since I started using openpilot 4 years ago.

I've done some digging after @sshane 's PCM compensation experiment and have found that the vehicle uses two acceleration values to calculate the amount of force that needs to be compensated, hence the ACCEL_CMD_ALT in all Toyota dbcs.

Side note, this ACCEL_CMD_ALT was originally named AT_RAW in the leaked file, which I believe is the DSU/camera's version of the un-compensated acceleration value, the vehicle only listens to ACCEL_CMD for actual controls, but it will still use ACCEL_CMD_ALT to calculate a compensatory force.

Basically, in openpilot terms, ACCEL_CMD_ALT, a.k.a.AT_RAW, is equivalent to openpilot's accel actuator (actuators.accel); and ACCEL_CMD, a.k.a. ATACC2, is equivalent to the min max acceleration clipped "openpilot accel actuator (actuators.accel) + compensatory force (CS.pcm_neutral_force) / vehicle weight (self.CP.mass)".

This PR addresses the following long-standing issues with Toyota longitudinal:

no more abruptions when reaching stopping state on TSS-P
no more slow accel request response
smooth SnG
no more cruising just below set speed when going downhill
no more unresponsive highway speed control

The following item(s) will need further verification:

[x] ~~smooth engagement and accelerator depression/release transition, try to achieve stock levels of smoothness when engaging, thanks for the feedback @AlexandreSato~~ should be okay now https://github.com/commaai/openpilot/pull/30697/commits/bdc00a5177bdd8b0900232fc39a3e23cfc52d231
[x] tuning, it seems like this tune works well on my compensated Prius, verify for other vehicles, especially those with a pedal, verify pedal transition
- [x] ~~According to @CT921 , some prius V users have noticed high speed oscillations, I've seen the same thing at around 60 km/h, fix this~~ should also be okay https://github.com/commaai/openpilot/pull/30697/commits/db93dbd88f85eadb501da6c5377bc780eb1304be
[x] ~~ret.vEgoStopping, verify if this value works on all vehicles, goal: vehicle shouldn't creep~~ works fine
[x] ~~ret.stoppingDecelRate = 0.8 can be optimised based on road slope to achieve a smoother stop, but is not necessary ret.stoppingDecelRate = 0.1 is very smooth~~ 0.3 okay, no creeping
[x] TSS 2.0, the way openpilot handles these cars is questionable, from what I've seen in Techstream, compensatory calculations will be paused when the vehicle is stopped and in standstill, but none of our TSS 2.0s go into standstill AFAIK, verify how this affects performance. TODO#1
[x] ~~Verify if PCM behaves consistently with gas press and lever (button) press when resuming. If PCM behaves differently, check if this is something that we can control to improve responsiveness.~~ behaves the same on my 2018 prius (non-phv)

Known issue: ~~1. TSS 2.0 vehicles will creep with the SnG hack enabled (which is default openpilot behaviour)~~ Current logic seems to have fixed this, needs more testing

To-dos:

[x] 1. ~~stop compensating on TSS 2.0 when stopping.~~
- [x] test on hills, flat surface route b4f83330b2d59205|2023-12-12--23-03-43--0, hill route: 7069d98b0de453c0/000000e4--f987d1b8d5/0
[x] 2. ~~Figure out how stock Toyota pulls away so quickly behind lead, does it disable compensation?~~ Seems like PERMIT_BRAKING can solve this.

Dec 12 '23 07:12 cydia2020

Nice! I'm gonna convert to draft until your TODOs are done.

Dec 12 '23 19:12 adeebshihadeh

Routes on a 2022 Camry Hybrid TSS2, has some interesting bookmarks: On commit 2635642:

b4f83330b2d59205|2023-12-12--23-03-43--0
b4f83330b2d59205|2023-12-13--13-57-47--0 (creeps at a red light in seg 14)

On commit a58db52:

b4f83330b2d59205|2023-12-14--08-45-29--0
b4f83330b2d59205|2023-12-14--17-10-29--0

Will edit this comment with more once I get some better examples, but it performed REALLY well and felt more like how I would control the throttle. Little to no oscillations on highway in experimental mode, too! Thanks for the awesome work!

Dec 13 '23 02:12 foodbandlt

Three consecutive bookmarks mean uphill, two downhill and one flat surface. Posted in redundancy in the Pull Request comment "Toyota Improve longitudinal control" and discord thread to gain traction.

On commit "Fix line too long" 62cc41a: 075b133b6181e058|2023-12-16--12-10-44 075b133b6181e058|2023-12-16--12-24-38 075b133b6181e058|2023-12-16--12-56-50 075b133b6181e058|2023-12-16--13-04-06

2023-12-16

[!TIP] Positives on my corolla TSS2 hybrid: No more abrupt accelerations, more responsive on uphill exits

[!CAUTION] Negative, it brakes unnecessarily when engaging.

Routes in dashcam mode (Toyota stock ACC): 075b133b6181e058|2023-12-16--20-01-47 075b133b6181e058|2023-12-16--20-33-46

Dec 16 '23 17:12 AlexandreSato

feels like a big improvement, tssp corolla pedal transition too. 62cc41a5b7343851092805f78e17e520437e4c48 routes

Dec 22 '23 12:12 ntegan1

As my previous stock routes were lost because I forgot to preserve, I'm posting new ones, better and this time preserved and public: 075b133b6181e058|2023-12-24--13-41-53 075b133b6181e058|2023-12-24--13-36-36

Dec 24 '23 19:12 AlexandreSato

on 5c789cb "adhd" It's no longer speeding up or slowing down. On highway: 075b133b6181e058|2023-12-27--12-17-49 city: 075b133b6181e058|2023-12-27--16-38-17

Dec 27 '23 20:12 AlexandreSato

On 77d3108 Brake when engage fixed! flawless absolutely no jerk or harsh just a bit of abrupt brake when entering in stopped state machine. Some routes: 075b133b6181e058|2023-12-28--14-12-50 075b133b6181e058|2023-12-28--15-27-40 075b133b6181e058|2023-12-28--17-51-12

Dec 28 '23 21:12 AlexandreSato

On 'revert these' 759f0c8: 075b133b6181e058|2024-01-05--12-03-55 (Public and preserved) Fixed hard braking when entering Stopped State. I believe creep happened here: 075b133b6181e058|2024-01-05--12-03-55--2 https://connect.comma.ai/075b133b6181e058/1704467167108/1704467176378

Jan 05 '24 17:01 AlexandreSato

In fact, what is happening is that STOP_DISTANCE is not consistent (see 075b133b6181e058|2024-01-06--16-23-56--4 at times it becomes undesirably very close and uncomfortable, it feels like it's going to hit the car in front). However, the longitudinal control by openpilot is now perfectly the same as stock Toyota, very good!

Jan 06 '24 19:01 AlexandreSato

Fixed the inconsistence on STOP_DISTANCE:

075b133b6181e058|2024-01-15--09-45-23 https://connect.comma.ai/075b133b6181e058/1705322723821/1705323760553

075b133b6181e058|2024-01-15--12-39-58 075b133b6181e058|2024-01-15--11-54-08

Jan 15 '24 16:01 AlexandreSato

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.

Jan 20 '24 13:01 github-actions[bot]

on: https://github.com/commaai/openpilot/commit/4bc10ba3309924dd54191073deb2ddf555e6b69e best longitudinal control seen in years. Only improvements without any regression on my corolla TSS2. Creep definitely solved! In my opinion it's good to go. Public and preserved route: https://connect.comma.ai/075b133b6181e058/1706014680345/1706016181471 075b133b6181e058|2024-01-23--09-58-02

Jan 23 '24 15:01 AlexandreSato

commit aa895f7282583cb092356cd8fe99c177fe7d79ee

a152dad3c3368320|2024-01-23--19-42-30 public 9 segment route similar experience with tssp corolla + pedal

Jan 24 '24 02:01 ntegan1

Also we might be able to fix #29613 once this is merged, as this does introduce a lead stopped condition, and that can be used to fix the standstill entrance behaviour

Currently my fork handles it like the following, and it mimicks stock ACC behaviour:

      if CS.out.vEgo < self.CP.vEgoStopping and lead_vehicle_stopped and self.CP.carFingerprint not in NO_STOP_TIMER_CAR:
        self.standstill_req = True
      else:
        self.standstill_req = False

Jan 25 '24 02:01 cydia2020

Also we might be able to fix #29613 once this is merged, as this does introduce a lead stopped condition, and that can be used to fix the standstill entrance behaviour

May I ask what you're trying to accomplish by considering the lead car's stopped condition? It's abstraction-breaking and I'm not sure what it gains you from reading this PR and other associated history. Does it improve the final stop position? Smoothness? Does it prevent creep on resume press? If there's some material improvement, then surely the same improvement is desired for stopping at a red light with no lead car visible.

Jan 25 '24 04:01 jyoung8607

Also we might be able to fix #29613 once this is merged, as this does introduce a lead stopped condition, and that can be used to fix the standstill entrance behaviour

May I ask what you're trying to accomplish by considering the lead car's stopped condition? It's abstraction-breaking and I'm not sure what it gains you from reading this PR and other associated history. Does it improve the final stop position? Smoothness? Does it prevent creep on resume press? If there's some material improvement, then surely the same improvement is desired for stopping at a red light with no lead car visible.

Yea it might not be needed tbh, anecdotally and from my own testing it seems to have helped with creeping

Jan 25 '24 06:01 cydia2020

Yea it might not be needed tbh, anecdotally and from my own testing it seems to have helped with creeping

To expand on what I was saying before: in an E2E world, policy decisions about how fast to go and when to stop/start really belong in higher levels of openpilot. However, if the Toyota CAN signal ACC_CONTROL.LEAD_VEHICLE_STOPPED is a real thing and the vehicle powertrain/chassis is verified to react to it in a way that we understand and find useful, the Toyota car port can absolutely (ab)use that signal to obtain desired stopping/starting performance. As one guess, stock TSS2 might use such a signal when the lead vehicle moves off, to let go of the brakes earlier and prepare for departure without actually applying engine throttle yet. There may be ways to use that to get openpilot to depart standstill faster from a green traffic light. I'm just guessing what its effect might be, I would have to observe the stock system working.

Jan 25 '24 07:01 jyoung8607

On 3b89906

Very good experience!

075b133b6181e058|2024-01-24--07-33-19

(Public and preserved) https://connect.comma.ai/075b133b6181e058/1706092401241/1706095112834

Jan 25 '24 19:01 AlexandreSato

good stops at https://github.com/commaai/openpilot/pull/30697/commits/ef15ad3d66bbf4fb9a3b9f3acdd9c9b4c3a9094b

075b133b6181e058|2024-01-26--10-39-08

Jan 26 '24 15:01 AlexandreSato

The stock system mostly keeps PERMIT_BRAKING at 1 when it's behind a lead, even during takeoff, so I think we can achieve better sng performance without touching it (I'd really like to avoid using it too since it's not easy to get a binary brake control bit like this to smoothly and safely work in all scenarios, especially on hills)

https://github.com/commaai/openpilot/assets/25857203/74878c55-ae4b-4efe-a670-eb3ceaf1cd87

Jan 29 '24 10:01 sshane

On the stock system of the 2021 Rav4 Prime it seems like ACCEL_CMD is always 0, and only ACCEL_CMD_ALT is used. (20ba9ade056a8c7b|2021-02-08--21-57-35). Is this the first car that shows this behavior?

Jan 29 '24 13:01 pd0wm

The stock system mostly keeps PERMIT_BRAKING at 1 when it's behind a lead, even during takeoff, so I think we can achieve better sng performance without touching it (I'd really like to avoid using it too since it's not easy to get a binary brake control bit like this to smoothly and safely work in all scenarios, especially on hills)

https://github.com/commaai/openpilot/assets/25857203/74878c55-ae4b-4efe-a670-eb3ceaf1cd87

Yup agreed, also I've realised that setting this bit to 0 at lower speeds does lead to some jerkiness when openpilot is letting go of the brakes, I'll get rid of it

Jan 30 '24 12:01 cydia2020

Good behavior no issues on

075b133b6181e058|2024-01-30--11-22-14

https://connect.comma.ai/075b133b6181e058/1706624534176/1706625197269 Eco and normal mode, public and preserved

075b133b6181e058|2024-01-30--11-01-25

preserved

Jan 30 '24 15:01 AlexandreSato

Very good at 'remove LVSTP from toyotacan and revert permit braking to stock' a8c3f6a13610996f1d34f98d4fbbeaddb4b123a8

075b133b6181e058|2024-02-02--21-10-12

075b133b6181e058|2024-02-02--19-11-04

Feb 03 '24 00:02 AlexandreSato

Hey! So I've jumped around stock Toyota long, OP long on master and OP long on this PR branch and this branch gets much closer to how stock Toyota long reacts with gas and brake (which is better than OP long). It's still much smoother than OP long on master, but seems to have gained some strange throttle oscillations that I haven't found the cause of yet.

Here's a preserved route on 5fee813 with bookmarks: b4f83330b2d59205|2024-02-08--22-44-39--0 (https://connect.comma.ai/b4f83330b2d59205/1707432279382/1707434769358)

Seg 9 has a very well executed hard stop
Segs 15-17 have starts, stops, and oscillations when stopping. Perhaps while starting too, I don't remember.
Seg 23 has oscillations when approaching stopped vehicle
Seg 24 has a nice hard stop
Seg 30 has oscillations at highway speeds when the lead slows down drastically
Seg 39 I believe had oscillations when approaching slower lead, but this is still by far the best braking performance I've had on any branch or fork!

Before the permit braking bit was removed, engaging while stopped behind a lead was broken and started idling forward with no intention of stopping. I'll test it again on this commit shortly and edit my comment.

Feb 09 '24 20:02 foodbandlt

Hey! So I've jumped around stock Toyota long, OP long on master and OP long on this PR branch and this branch gets much closer to how stock Toyota long reacts with gas and brake (which is better than OP long). It's still much smoother than OP long on master, but seems to have gained some strange throttle oscillations that I haven't found the cause of yet.

Here's a preserved route on 5fee813 with bookmarks: b4f83330b2d59205|2024-02-08--22-44-39--0 (https://connect.comma.ai/b4f83330b2d59205/1707432279382/1707434769358)

Seg 9 has a very well executed hard stop

Segs 15-17 have starts, stops, and oscillations when stopping. Perhaps while starting too, I don't remember.

Seg 23 has oscillations when approaching stopped vehicle

Seg 24 has a nice hard stop

Seg 30 has oscillations at highway speeds when the lead slows down drastically

Seg 39 I believe had oscillations when approaching slower lead, but this is still by far the best braking performance I've had on any branch or fork!

Before the permit braking bit was removed, engaging while stopped behind a lead was broken and started idling forward with no intention of stopping. I'll test it again on this commit shortly and edit my comment.

Throttle oscillation is usually tuning related, I've noticed it on mine as well when decelerating over a distance downhill. Tuning is something that I'll need to look into a bit more, as of now I can still identify some imperfections with stock MPC, hopefully tuning can reduce those.

Feb 09 '24 21:02 cydia2020

Very good at bf3018e](https://github.com/commaai/openpilot/pull/30697/commits/bf3018e3d36809c908d144d45cc85994730a29da) (2024-02-14) Public and preserved test routes

075b133b6181e058|2024-02-14--11-43-21

https://connect.comma.ai/075b133b6181e058/1707921801231/1707922285843

Feb 14 '24 15:02 AlexandreSato

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

Mar 18 '24 01:03 github-actions[bot]

This PR has been automatically closed due to inactivity. Feel free to re-open once activity resumes.

Mar 25 '24 01:03 github-actions[bot]

Thanks for contributing to openpilot! In order for us to review your PR as quickly as possible, check the following:

Convert your PR to a draft unless it's ready to review
Read the contributing docs
Before marking as "ready for review", ensure:
- the goal is clearly stated in the description
- all the tests are passing
- the change is something we merge
- include a route or your device' dongle ID if relevant

Mar 25 '24 02:03 github-actions[bot]