Send future accel to car (eliminate jerking) (commaai#408)

* send future accel (0.45 seconds) to car as feedforward, not changing velocity * add comment for future me * add comment for future me * Revert "Disable long derivative if eager accel is enabled (same thing)" This reverts commit dc785ce. * Disable long deriv * Revert "Experimental feature: eager accel (commaai#382)" This reverts commit 58ee449. * rename back * offset but keep gas so car can release brakes easier * just offset * Update version * Update README.md * Update README.md * Update README.md * don't change tuning * add CP for accel delay * clip * always get that confused * notes * Update date * Add param to tune * Add param to tune
LexusRXopenpilotUG · May 3, 2021 · 753ee2b · 753ee2b
1 parent 5dff648
commit 753ee2b
Show file tree

Hide file tree

Showing 10 changed files with 52 additions and 231 deletions.
diff --git a/README.md b/README.md
@@ -1,4 +1,4 @@
-# Stock Additions [Update 2](/SA_RELEASES.md) (0.8.2)
+# Stock Additions [Update 3](/SA_RELEASES.md) (0.8.2)
 
 Stock Additions is a fork of openpilot designed to be minimal in design while boasting various feature additions and behavior improvements over stock. I have a 2017 Toyota Corolla with comma pedal, so most of my changes are designed to improve the longitudinal performance.
 
@@ -19,7 +19,7 @@ Want to request a feature or create a bug report? [Open an issue here!](https://
 * [**Adding derivative to PI for better control**](#pi---pid-controller-for-long-and-lat) - lat: smoother control in turns; long: fix for comma pedal overshoot
 
 ### General Features
-* [**NEW❗ Eager acceleration**](#new-eager-acceleration-experimental-feature) - using the jerk of desired accel to respond quicker to braking and acceleration
+* [**NEW❗ Smoother long control using delay**](#new-compensate-for-longitudinal-delay-for-earlier-braking) - using an accel delay, just like for lateral
 * [**Customize this fork**](#Customize-this-fork-opEdit) - easily edit fork parameters with support for live tuning
 * [**Automatic updates**](#Automatic-updates)
 * [**ZSS Support**](#ZSS-support) - takes advantage of your high-precision Zorrobyte Steering Sensor
@@ -116,20 +116,10 @@ If you have a car without a pedal, or you do have one but I haven't created a pr
 ---
 ## General Features
 
-### NEW❗ Eager acceleration *(experimental feature)*
-For some Toyota cars (primarily on older TSS1's like the '17 Corolla), there seems to exist some hysteresis in the ACC (adaptive cruise control) system. This can be most noticed when openpilot requests some brake shortly after accelerating, it's quite delayed causing integral to wind up and jerking to ensue when the lead comes to a stop, even if they're rather smooth about it.
+### NEW❗ Compensate for longitudinal delay for earlier braking
+This just simply uses desired future acceleration for feedforward rather than current desired acceleration. openpilot already compensates for steering delay, but not longitudinal. This adds that, replacing the previous ***experimental*** feature called eager accel which tried to fix the same issues; jerky and late braking. Now we more correctly compensate for delay.
 
-This feature aims to try and combat that by modifying the final acceleration sent to the car, it sums the regular desired acceleration, and the jerk of the desired acceleration (the derivative of the derivative) so that we can react quicker to changes in direction of acceleration.
-
-To try it, edit the `eager_accel` param with opEdit to set it to the integer `2`. There's also another method that uses the derivative of the desired acceleration, but it's not ideal since it can pretty severely change the desired acceleration value when it's changing linearly, and not changing direction. The jerk method is probably going to be the one I keep around once this feature is out of beta.
-
-To adjust the tuning for this feature, you can change the `accel_eagerness` parameter, this is simply a multiplier for the modification (`apply_accel += eager_mod * accel_eagerness`).
-
-Here's a quick graph how the two methods compare, you can see how the derivative method warps the acceleration much further than jerk.
-
-<p align="center">
-  <img src=".media/eager_accel.png?raw=true">
-</p>
+By default, we assume a 0.4 second delay from sending acceleration to seeing it realized, which is tunable with the opEdit param `long_accel_delay`. Raise if braking too late, lower if braking too early. Stock openpilot is 0.0 (no delay).
 
 ---
 ### Customize this fork (opEdit)

diff --git a/SA_RELEASES.md b/SA_RELEASES.md
@@ -1,3 +1,8 @@
+Stock Additions Update 3 - 2021-05-03 (0.8.2)
+===
+ * Remove eager accel and instead send future desired accel
+   * Much simpler and better/smoother results (tunable with `long_accel_delay`)
+
 Stock Additions Update 2 - 2021-04-21 (0.8.2)
 ===
  * Update versioning sceme to *Updates*, which reset on each new openpilot version

diff --git a/cereal/car.capnp b/cereal/car.capnp
@@ -406,6 +406,7 @@ struct CarParams {
   startingBrakeRate @53 :Float32; # brake_travel/s while releasing on restart
 
   steerActuatorDelay @36 :Float32; # Steering wheel actuator delay in seconds
+  longAccelDelay @39 :Float32; # Acceleration delay in seconds
   openpilotLongitudinalControl @37 :Bool; # is openpilot doing the longitudinal control?
   carVin @38 :Text; # VIN number queried during fingerprinting
   dashcamOnly @41: Bool;

diff --git a/common/op_params.py b/common/op_params.py
@@ -99,20 +99,8 @@ def __init__(self):
 
     self.fork_params = {'camera_offset': Param(0.06, NUMBER, 'Your camera offset to use in lane_planner.py', live=True),
                         'dynamic_follow': Param('auto', str, static=True, hidden=True),
-                        'eager_accel': Param(None, [type(None), int], 'Experimental❗ Combats hysteresis in the cruise control system, braking sooner to eliminate jerking\n'
-                                                                      'Might only work for TSS1 Toyotas\n'
-                                                                      'Set the param to `1` to use the first method: uses the smoothened derivative of desired accel\n'
-                                                                      'Setting the param to `2` uses the smoothened jerk of acceleration (tighter control/modification)\n'
-                                                                      'Try out both and see which is smoother. None disables this feature', live=True),
-                        'accel_eagerness': Param(1.0, NUMBER, 'Multiplier for the acceleration modifier. 1 is full eagerness (default), 0.8 is 80%, etc.', live=True),
-
-                        'accel_time_constant_0_mph': Param(0.01, NUMBER, 'Time constant for eager accel at 0 mph in seconds\n'  # todo: tune using these then remove
-                                                                         'Approaching 0 is no change, the higher it is the more the response\n'
-                                                                         'The defaults are generally okay', live=True),
-                        'accel_time_constant_10_mph': Param(0.1, NUMBER, 'Time constant for eager accel at 10 mph in seconds', live=True),
-                        'accel_time_constant_80_mph': Param(1.0, NUMBER, 'Time constant for eager accel at 80 mph in seconds', live=True),
-
-                        # 'replay_accel': Param(False, bool, 'Set param to True while engaged at 20 mph', live=True),
+                        'long_accel_delay': Param(0.4, NUMBER, 'The delay in seconds from requesting acceleration to seeing it\n'
+                                                               'Raise if braking late, lower if braking early. Stock openpilot uses 0.0', live=True),
                         'global_df_mod': Param(1.0, NUMBER, 'The multiplier for the current distance used by dynamic follow. The range is limited from 0.85 to 2.5\n'
                                                             'Smaller values will get you closer, larger will get you farther\n'
                                                             'This is multiplied by any profile that\'s active. Set to 1. to disable', live=True),
@@ -124,8 +112,8 @@ def __init__(self):
                         # 'lane_speed_alerts': Param('silent', str, 'Can be: (\'off\', \'silent\', \'audible\')\n'
                         #                                           'Whether you want openpilot to alert you of faster-traveling adjacent lanes'),
                         'upload_on_hotspot': Param(False, bool, 'If False, openpilot will not upload driving data while connected to your phone\'s hotspot'),
-                        'enable_long_derivative': Param(False, bool, 'If you have longitudinal overshooting, enable this! This enables derivative-based\n'
-                                                                     'integral wind-down to help reduce overshooting within the long PID loop'),
+                        # 'enable_long_derivative': Param(False, bool, 'If you have longitudinal overshooting, enable this! This enables derivative-based\n'
+                        #                                              'integral wind-down to help reduce overshooting within the long PID loop'),
                         'disengage_on_gas': Param(False, bool, 'Whether you want openpilot to disengage on gas input or not'),
                         'update_behavior': Param('auto', str, 'Can be: (\'off\', \'alert\', \'auto\') without quotes\n'
                                                               'off will never update, alert shows an alert on-screen\n'

diff --git a/misc_scripts/debug/explore_eager.py b/misc_scripts/debug/explore_eager.py
diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py
@@ -64,6 +64,8 @@ def get_std_params(candidate, fingerprint, has_relay):
     ret.minSteerSpeed = 0.
 
     # stock ACC by default
+    ret.longAccelDelay = 0.4
+
     ret.enableCruise = True
     ret.minEnableSpeed = -1.  # enable is done by stock ACC, so ignore this
     ret.steerRatioRear = 0.  # no rear steering, at least on the listed cars aboveA

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
@@ -1,6 +1,5 @@
 from cereal import car
 from common.numpy_fast import clip, interp
-from common.realtime import DT_CTRL
 from selfdrive.car import apply_toyota_steer_torque_limits, create_gas_command, make_can_msg
 from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_command, \
                                            create_accel_command, create_acc_cancel_command, \
@@ -48,10 +47,7 @@ def __init__(self, dbc_name, CP, VM):
     self.alert_active = False
     self.last_standstill = False
     self.standstill_req = False
-
-    self.op_params = opParams()
-    self.standstill_hack = self.op_params.get('standstill_hack')
-    self.reset_eager()
+    self.standstill_hack = opParams().get('standstill_hack')
 
     self.steer_rate_limited = False
 
@@ -63,10 +59,6 @@ def __init__(self, dbc_name, CP, VM):
 
     self.packer = CANPacker(dbc_name)
 
-  def reset_eager(self):
-    self.delayed_accel = 0.
-    self.delayed_derivative = 0.
-
   def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
              left_line, right_line, lead, left_lane_depart, right_lane_depart):
 
@@ -81,25 +73,7 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
       # +0.06 offset to reduce ABS pump usage when applying very small gas
       if apply_accel * CarControllerParams.ACCEL_SCALE > coast_accel(CS.out.vEgo):
         apply_gas = clip(compute_gb_pedal(apply_accel * CarControllerParams.ACCEL_SCALE, CS.out.vEgo), 0., 1.)
-
-
-    eager_accel_method = self.op_params.get('eager_accel')
-    if eager_accel_method in [1, 2]:
-      if not enabled:  # reset states
-        self.reset_eager()
-
-      y = [self.op_params.get(p) for p in ['accel_time_constant_0_mph', 'accel_time_constant_10_mph', 'accel_time_constant_80_mph']]
-      RC = interp(CS.out.vEgo, [0, 5, 35], y)
-      alpha = 1. - DT_CTRL / (RC + DT_CTRL)
-      self.delayed_accel = self.delayed_accel * alpha + apply_accel * (1. - alpha)
-
-      eagerness = self.op_params.get('accel_eagerness')
-      if eager_accel_method == 1:  # new accel is simply accel - change in accel over exponential time (time constant varies with speed)
-        apply_accel = apply_accel - (self.delayed_accel - apply_accel) * eagerness
-      else:  # subtracting difference in smoothened accel derivative and current derivative (jerk, takes one more variable to keep track of derivative over time but control is more tight)
-        derivative = apply_accel - self.delayed_accel  # store change in accel over some time constant (using exponential moving avg.)
-        self.delayed_derivative = self.delayed_derivative * alpha + derivative * (1. - alpha)  # calc exp. moving average for derivative
-        apply_accel = apply_accel - (self.delayed_derivative - derivative) * eagerness  # then modify accel using jerk of accel
+      apply_accel += 0.06
 
     apply_accel, self.accel_steady = accel_hysteresis(apply_accel, self.accel_steady, enabled)
     apply_accel = clip(apply_accel * CarControllerParams.ACCEL_SCALE, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)

diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h
@@ -1 +1 @@
-#define COMMA_VERSION "0.8.2 - SA Update 2"
+#define COMMA_VERSION "0.8.2 - SA Update 3"
Original file line number	Diff line number	Diff line change
		@@ -1 +1 @@
		#define COMMA_VERSION "0.8.2 - SA Update 2"
		#define COMMA_VERSION "0.8.2 - SA Update 3"