use liveParams pitch compensation per commaai#25469

bounds check

use correct pitch bounds and rates

selfdrive/car/gm/carcontroller.py

@@ -11,6 +11,9 @@
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 
+# only use pitch-compensated acceleration at 10m/s+
+ACCEL_PITCH_FACTOR_BP = [5., 10.] # [m/s]
+ACCEL_PITCH_FACTOR_V = [0., 1.] # [unitless in [0-1]]
 
 class CarController():
   def __init__(self, dbc_name, CP, VM):
@@ -58,20 +61,15 @@ def update(self, enabled, CS, frame, actuators,
       can_sends.append(gmcan.create_steering_control(self.packer_pt, CanBus.POWERTRAIN, apply_steer, idx, lkas_enabled))
 
     # Gas/regen prep
-    # apply pitch "shifted deadzone" that gives smooth output in addition to applying a deadzone
-    if abs(CS.pitch_accel) > CS.pitch_accel_deadzone:
-      pitch = CS.pitch_accel + (CS.pitch_accel_deadzone if CS.pitch_accel < 0. else -CS.pitch_accel_deadzone)
-    else:
-      pitch = 0.
-    gravity_x = -9.8 * sin(pitch) * CS.pitch_accel_factor
     if not enabled or CS.pause_long_on_gas_press:
       # Stock ECU sends max regen when not enabled.
       apply_gas = P.MAX_ACC_REGEN
       apply_brake = 0
     else:
-      apply_gas = interp(actuators.accel - gravity_x, P.GAS_LOOKUP_BP, P.GAS_LOOKUP_V)
-      pitch_brake_lowspeed_factor = interp(CS.vEgo, CS.pitch_accel_brake_lowspeed_lockout_bp, CS.pitch_accel_brake_lowspeed_lockout_v)
-      apply_brake = interp(actuators.accel - gravity_x * pitch_brake_lowspeed_factor, P.BRAKE_LOOKUP_BP, P.BRAKE_LOOKUP_V)
+      k = interp(CS.out.vEgo, ACCEL_PITCH_FACTOR_BP, ACCEL_PITCH_FACTOR_V)
+      brake_accel = k * actuators.accelPitchCompensated + (1. - k) * actuators.accel
+      apply_gas = interp(actuators.accelPitchCompensated, P.GAS_LOOKUP_BP, P.GAS_LOOKUP_V)
+      apply_brake = interp(brake_accel, P.BRAKE_LOOKUP_BP, P.BRAKE_LOOKUP_V)
       t = sec_since_boot()
 
       v_rel = CS.coasting_lead_v - CS.vEgo

selfdrive/car/gm/carstate.py

@@ -125,13 +125,6 @@ def __init__(self, CP):
     self.pitch_raw = 0. # radians
     self.pitch_ema = 1/100
     self.pitch_future_time = 0.5 # seconds
-    self.pitch_accel_factor = 0.85
-    self.pitch_accel_deadzone = 0.01 # radians ~ ±1% grade
-    self.pitch_accel = 0.
-    self.pitch_accel_raw = 0.
-    self.pitch_accel_future_time = 0.8
-    self.pitch_accel_brake_lowspeed_lockout_bp = [i * CV.MPH_TO_MS for i in [5., 15.]]
-    self.pitch_accel_brake_lowspeed_lockout_v = [0., 1.]
 
 
     # similar to over-speed coast braking, lockout coast/one-pedal logic first for engine/regen braking, and then for actual brakes.
@@ -361,7 +354,6 @@ def update(self, pt_cp, loopback_cp):
     ret.onePedalBrakeMode = self.one_pedal_brake_mode
 
     self.pitch = self.pitch_ema * self.pitch_raw + (1 - self.pitch_ema) * self.pitch 
-    self.pitch_accel = self.pitch_ema * self.pitch_accel_raw + (1 - self.pitch_ema) * self.pitch_accel
     ret.pitch = self.pitch
 
     ret.autoHoldActivated = self.autoHoldActivated

selfdrive/controls/controlsd.py

@@ -16,14 +16,17 @@
 from selfdrive.controls.lib.lane_planner import CAMERA_OFFSET
 from selfdrive.controls.lib.drive_helpers import update_v_cruise, initialize_v_cruise, V_CRUISE_MIN
 from selfdrive.controls.lib.drive_helpers import get_lag_adjusted_curvature
+from selfdrive.controls.lib.drive_helpers import apply_deadzone
 from selfdrive.controls.lib.longcontrol import LongControl, STARTING_TARGET_SPEED
+from selfdrive.controls.lib.lane_planner import TRAJECTORY_SIZE
 from selfdrive.controls.lib.latcontrol_pid import LatControlPID
 from selfdrive.controls.lib.latcontrol_indi import LatControlINDI
 from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR
 from selfdrive.controls.lib.latcontrol_angle import LatControlAngle
 from selfdrive.controls.lib.latcontrol_torque import LatControlTorque
 from selfdrive.controls.lib.events import Events, ET
 from selfdrive.controls.lib.alertmanager import AlertManager
+from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
 from selfdrive.controls.lib.vehicle_model import VehicleModel
 from selfdrive.locationd.calibrationd import Calibration
 from selfdrive.modeld.constants import T_IDXS
@@ -171,6 +174,8 @@ def __init__(self, sm=None, pm=None, can_sock=None):
     self.logged_comm_issue = False
     self.v_target = 0.0
     self.a_target = 0.0
+    self.pitch = 0.0
+    self.pitch_accel_deadzone = 0.01 # [radians] ≈ 1% grade
 
     # TODO: no longer necessary, aside from process replay
     self.sm['liveParameters'].valid = True
@@ -539,12 +544,9 @@ def state_control(self, CS):
     lat_plan = self.sm['lateralPlan']
     long_plan = self.sm['longitudinalPlan']
 
-    if self.sm.valid.get('liveLocationKalman', False) and self.sm.valid.get('modelV2', False) and len(self.sm['modelV2'].orientation.y) >= 12 and len(self.sm['liveLocationKalman'].calibratedOrientationNED.value) > 1 and sec_since_boot() > 120.:
-      current_pitch = clip(self.sm['liveLocationKalman'].calibratedOrientationNED.value[1], -MAX_ABS_PITCH, MAX_ABS_PITCH)
+    if self.sm.updated['liveParameters'] and len(self.sm['modelV2'].orientation.y) == TRAJECTORY_SIZE:
       future_pitch_diff = clip(interp(self.CI.CS.pitch_future_time, T_IDXS, self.sm['modelV2'].orientation.y), -MAX_ABS_PRED_PITCH_DELTA, MAX_ABS_PRED_PITCH_DELTA)
-      self.CI.CS.pitch_raw = current_pitch + future_pitch_diff
-      future_pitch_diff = clip(interp(self.CI.CS.pitch_accel_future_time, T_IDXS, self.sm['modelV2'].orientation.y), -MAX_ABS_PRED_PITCH_DELTA, MAX_ABS_PRED_PITCH_DELTA)
-      self.CI.CS.pitch_accel_raw = current_pitch + clip(future_pitch_diff, -MAX_ABS_PRED_PITCH_DELTA, MAX_ABS_PRED_PITCH_DELTA)
+      self.CI.CS.pitch_raw = self.sm['liveParameters'].pitch + future_pitch_diff
 
 
     actuators = car.CarControl.Actuators.new_message()
@@ -570,6 +572,12 @@ def state_control(self, CS):
       t_since_plan = (self.sm.frame - self.sm.rcv_frame['longitudinalPlan']) * DT_CTRL
       actuators.accel = self.LoC.update(self.active, CS, self.CP, long_plan, pid_accel_limits, t_since_plan)
 
+      # compute pitch-compensated accel
+      if self.sm.updated['liveParameters']:
+        self.pitch = apply_deadzone(self.sm['liveParameters'].pitchFutureLong, self.pitch_accel_deadzone)
+      actuators.accelPitchCompensated = actuators.accel + ACCELERATION_DUE_TO_GRAVITY * math.sin(self.pitch)
+
+
       self.CI.CS.coasting_long_plan = long_plan.longitudinalPlanSource
       self.CI.CS.coasting_lead_d = long_plan.leadDist
       self.CI.CS.coasting_lead_v = long_plan.leadV

selfdrive/controls/lib/drive_helpers.py

@@ -43,6 +43,14 @@ class MPC_COST_LONG:
   ACCELERATION = 10.0
   JERK = 20.0
 
+def apply_deadzone(error, deadzone):
+  if error > deadzone:
+    error -= deadzone
+  elif error < - deadzone:
+    error += deadzone
+  else:
+    error = 0.
+  return error
 
 def rate_limit(new_value, last_value, dw_step, up_step):
   return clip(new_value, last_value + dw_step, last_value + up_step)

selfdrive/locationd/models/car_kf.py

@@ -39,6 +39,7 @@ class States():
   YAW_RATE = _slice(1)  # [rad/s]
   STEER_ANGLE = _slice(1)  # [rad]
   ROAD_ROLL = _slice(1)  # [rad]
+  ROAD_PITCH = _slice(1)  # [rad]
 
 
 class CarKalman(KalmanFilter):
@@ -53,6 +54,7 @@ class CarKalman(KalmanFilter):
     10.0, 0.0,
     0.0,
     0.0,
+    0.0,
     0.0
   ])
 
@@ -67,13 +69,15 @@ class CarKalman(KalmanFilter):
     math.radians(0.1)**2,
     math.radians(0.1)**2,
     math.radians(1)**2,
+    math.radians(1)**2,
   ])
   P_initial = Q.copy()
 
   obs_noise: Dict[int, Any] = {
     ObservationKind.STEER_ANGLE: np.atleast_2d(math.radians(0.05)**2),
     ObservationKind.ANGLE_OFFSET_FAST: np.atleast_2d(math.radians(10.0)**2),
     ObservationKind.ROAD_ROLL: np.atleast_2d(math.radians(1.0)**2),
+    ObservationKind.ROAD_PITCH: np.atleast_2d(math.radians(1.0)**2),
     ObservationKind.STEER_RATIO: np.atleast_2d(5.0**2),
     ObservationKind.STIFFNESS: np.atleast_2d(0.5**2),
     ObservationKind.ROAD_FRAME_X_SPEED: np.atleast_2d(0.1**2),
@@ -112,6 +116,7 @@ def generate_code(generated_dir):
     angle_offset = state[States.ANGLE_OFFSET, :][0, 0]
     angle_offset_fast = state[States.ANGLE_OFFSET_FAST, :][0, 0]
     theta = state[States.ROAD_ROLL, :][0, 0]
+    phi = state[States.ROAD_PITCH, :][0, 0]
     sa = state[States.STEER_ANGLE, :][0, 0]
 
     sR = state[States.STEER_RATIO, :][0, 0]
@@ -156,6 +161,7 @@ def generate_code(generated_dir):
       [sp.Matrix([sR]), ObservationKind.STEER_RATIO, None],
       [sp.Matrix([sf]), ObservationKind.STIFFNESS, None],
       [sp.Matrix([theta]), ObservationKind.ROAD_ROLL, None],
+      [sp.Matrix([phi]), ObservationKind.ROAD_PITCH, None],
     ]
 
     gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, global_vars=global_vars)

selfdrive/locationd/models/constants.py

@@ -39,6 +39,7 @@ class ObservationKind:
   STEER_RATIO = 29  # [-]
   ROAD_FRAME_X_SPEED = 30  # (x) [m/s]
   ROAD_ROLL = 31  # [rad]
+  ROAD_PITCH = 34  # [rad]
 
   names = [
     'Unknown',
@@ -72,6 +73,7 @@ class ObservationKind:
     'Steer Ratio',
     'Road Frame x speed',
     'Road Roll',
+    'Road Pitch',
   ]
 
   @classmethod

selfdrive/locationd/paramsd.py

@@ -9,15 +9,19 @@
 from cereal import car
 from common.params import Params, put_nonblocking
 from common.realtime import set_realtime_priority, DT_MDL
-from common.numpy_fast import clip
+from common.numpy_fast import clip, interp
+from selfdrive.controls.lib.lane_planner import TRAJECTORY_SIZE
 from selfdrive.locationd.models.car_kf import CarKalman, ObservationKind, States
 from selfdrive.locationd.models.constants import GENERATED_DIR
+from selfdrive.modeld.constants import T_IDXS
 from selfdrive.swaglog import cloudlog
 
 
 MAX_ANGLE_OFFSET_DELTA = 20 * DT_MDL  # Max 20 deg/s
-ROLL_MAX_DELTA = np.radians(20.0) * DT_MDL  # 20deg in 1 second is well within curvature limits
+ROLL_MAX_DELTA = math.radians(20.0) * DT_MDL  # 20deg in 1 second is well within curvature limits
 ROLL_MIN, ROLL_MAX = math.radians(-10), math.radians(10)
+PITCH_MAX_DELTA = math.radians(10.0) * DT_MDL  # 10°/s
+PITCH_MIN, PITCH_MAX = math.radians(-19), math.radians(19) # steepest roads in US are ~18°
 
 class ParamsLearner:
   def __init__(self, CP, steer_ratio, stiffness_factor, angle_offset, P_initial=None):
@@ -34,8 +38,13 @@ def __init__(self, CP, steer_ratio, stiffness_factor, angle_offset, P_initial=No
 
     self.speed = 0.0
     self.roll = 0.0
+    self.pitch = 0.0
     self.steering_pressed = False
     self.steering_angle = 0.0
+
+    self.future_pitch_delay = 0.4 + (CP.longitudinalActuatorDelayUpperBound + CP.longitudinalActuatorDelayLowerBound) / 2
+    self.future_pitch_ema_k = 1/10
+    self.future_pitch_ema = 0.0
 
     self.valid = True
 
@@ -56,6 +65,19 @@ def handle_log(self, t, which, msg):
         roll = 0.0
         roll_std = np.radians(10.0)
       self.roll = clip(roll, self.roll - ROLL_MAX_DELTA, self.roll + ROLL_MAX_DELTA)
+
+      localizer_pitch = msg.orientationNED.value[1]
+      localizer_pitch_std = np.radians(1) if np.isnan(msg.orientationNED.std[1]) else msg.orientationNED.std[1]
+      pitch_valid = msg.orientationNED.valid and PITCH_MIN < localizer_pitch < PITCH_MAX
+      if pitch_valid:
+        pitch = localizer_pitch
+        # Experimentally found multiplier of 2 to be best trade-off between stability and accuracy or similar?
+        pitch_std = 2 * localizer_pitch_std
+      else:
+        # This is done to bound the road pitch estimate when localizer values are invalid
+        pitch = 0.0
+        pitch_std = np.radians(10.0)
+      self.pitch = clip(pitch, self.pitch - PITCH_MAX_DELTA, self.pitch + PITCH_MAX_DELTA)
 
       yaw_rate_valid = msg.angularVelocityCalibrated.valid
       yaw_rate_valid = yaw_rate_valid and 0 < yaw_rate_std < 10  # rad/s
@@ -74,6 +96,12 @@ def handle_log(self, t, which, msg):
                                       ObservationKind.ROAD_ROLL,
                                       np.array([[self.roll]]),
                                       np.array([np.atleast_2d(roll_std**2)]))
+
+          self.kf.predict_and_observe(t,
+                                      ObservationKind.ROAD_PITCH,
+                                      np.array([[self.pitch]]),
+                                      np.array([np.atleast_2d(pitch_std**2)]))
+
         self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, np.array([[0]]))
 
         # We observe the current stiffness and steer ratio (with a high observation noise) to bound
@@ -95,6 +123,12 @@ def handle_log(self, t, which, msg):
       if self.active:
         self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[math.radians(msg.steeringAngleDeg)]]))
         self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_X_SPEED, np.array([[self.speed]]))
+    elif which == 'modelV2' and self.active and len(msg.orientation.y) == TRAJECTORY_SIZE:
+      future_pitch_diff = interp(self.future_pitch_delay, T_IDXS, msg.orientation.y)
+      future_pitch = float(self.kf.x[States.ROAD_PITCH]) + future_pitch_diff
+      future_pitch = clip(future_pitch, self.future_pitch_ema - PITCH_MAX_DELTA, self.future_pitch_ema + PITCH_MAX_DELTA)
+      self.future_pitch_ema = self.future_pitch_ema_k * future_pitch \
+                            + (1 - self.future_pitch_ema_k) * self.future_pitch_ema
 
     if not self.active:
       # Reset time when stopped so uncertainty doesn't grow
@@ -107,7 +141,7 @@ def main(sm=None, pm=None):
   set_realtime_priority(5)
 
   if sm is None:
-    sm = messaging.SubMaster(['liveLocationKalman', 'carState'], poll=['liveLocationKalman'])
+    sm = messaging.SubMaster(['liveLocationKalman', 'carState', 'modelV2'], poll=['liveLocationKalman'])
   if pm is None:
     pm = messaging.PubMaster(['liveParameters'])
 
@@ -184,6 +218,8 @@ def main(sm=None, pm=None):
       msg.liveParameters.steerRatio = float(x[States.STEER_RATIO])
       msg.liveParameters.stiffnessFactor = float(x[States.STIFFNESS])
       msg.liveParameters.roll = float(x[States.ROAD_ROLL])
+      msg.liveParameters.pitch = float(x[States.ROAD_PITCH])
+      msg.liveParameters.pitchFutureLong = float(learner.future_pitch_ema)
       msg.liveParameters.angleOffsetAverageDeg = angle_offset_average
       msg.liveParameters.angleOffsetDeg = angle_offset
       msg.liveParameters.valid = all((