GM long: add pitch-compensation #25485

twilsonco · 2022-08-19T00:31:45Z

(This, but without model-predicted pitch, and this is entirely contained in gm/carcontroller)

Description/justification

This PR adds pitch compensation to longitudinal control for GM cars, resulting in a large decrease in longitudinal error (versus the long plan) when pitch changes or when on an incline/decline.

Adds longitudinal pitch compensation to be used in cars that use gas/brake control, where the command specifies an amount of acceleration force rather than an amount of acceleration relative to the road. The implementation is contained in car controller, as a type of passive long feedforward, transparent to the controller.

To allow this to account for longitudinal actuator delay to allow for smoothing of the pitch signal, future model pitch is used to create a smoothed, predictive pitch.

Though not shown here, another benefit of this improvement is that if more than ACCEL_MAX acceleration is required to maintain speed up a steep hill, stock will not be able to maintain speed. With this improvement that is no longer the case.

actuators.accelPitchCompensated is used to determine the gas command, which specifies an acceleration force. The brake command on gm specifies acceleration relative to the ground, so the actuators.accel value is the correct value for friction brakes, even when going downhill.
To support this assertion, see that gas command does not match up with aEgo:

While brake command does match with aEgo

I also verified this extensively (though not with proper documentation) when developing "one pedal mode" in my openpilot fork, which involved artificially sending a constant brake command and observing the car's braking behavior. I found that the same brake command produces far more braking force on a decline, and far less (or none) on an incline.

However, when openpilot requests negative acceleration in order to stop behind a lead when going uphill, it would engage the friction brakes sooner (at the same time it would now, without this improvement), despite the negative gravitational acceleration, and resulting in more braking than in necessary, since it's being assisted by gravity.
So you'd like for openpilot to postpone friction brake use when decelerating uphill, but it can become a problem when stopping, as the pitch-compensated acceleration may remain positive even though openpilot would like to stop, resulting in late stopping behind leads.
The solution is to smoothly switch to non-pitch-compensated acceleration for determining brakes at low speed.
I've been using this method on my fork for about a month and it keeps the efficiency and brake-avoidance benefits of pitch-compensation without introducing possible problems when stopping behind leads.

(Any issues caused by the gas and brake signals having different physical meanings will also be minimized by using the correct gas/brake threshold acceleration value)

This should also be used on other makes that specify acceleration force with gas or brake commands

Route: c11fcb510a549332|2022-08-19--09-03-54--0

twilsonco · 2022-08-19T18:00:20Z

Route: c11fcb510a549332|2022-08-19--09-03-54--0

Verification

Here we compare master (no pitch) with present and future pitch compensation on a couple steep steps of a hill, going up and down the steps. "Present pitch" is from the above route.

Pitch compensation cuts speed error in half, and lowers acceleration error by 13%.
Additionally, this eliminates the problem of MAX_ACCEL (2m/s^2) being insufficient for climbing steep hills; with pitch compensation op can accelerate up any hill the car can.

(In the future, when Comma has sufficiently verified the model predicted pitch, I'd like to include future pitch in order to compensate for long actuator delay, which, as you can see here, decreases speed and acceleration error by an additional 46% compared to when using instantaneous pitch, and is tunable depending on the value of long actuator delay. Compared to master, future pitch compensation reduces speed and acceleration error by 74% and 53% respectively, resulting in greater passenger comfort and improved vehicle efficiency.)

comment don't initialize filter log and use new_actuators.accel calculate accel_g for all frames record smoothed pitch to pitchDEPRECATED do initialize filter indentation no save deprecatedPitch comment

sshane · 2023-08-17T03:06:42Z

It's possible we need something similar to this for Toyotas to undo the accel compensation the PCM does, so we should try to make this as generic as possible.

twilsonco · 2023-08-17T21:17:56Z

So something in between this and the previous implementation that was unnecessarily couched inside of liveParams?

adeebshihadeh · 2023-08-17T21:24:20Z

This isn't that large; blocking this on abstracting it out seems like premature optimization.

twilsonco · 2023-08-17T22:12:54Z

The additional calculation of accel_g in the else block necessary at all. If that's not there, then accel is only changed at the lower rate but logged at 100Hz, making the data appear choppy in logs.